﻿/*
 * @author: wizardc
 */

using System.Collections.Generic;
using UnityEngine;

namespace Dou.BoundingVolumeHierarchy
{
    /// <summary>
    /// BVH 构建和查找类
    /// 改自开源项目：https://github.com/EmmetOT/BoundingVolumeHierarchy，算法来自box2d
    /// 采用二叉树的结构，内部使用AVL算法使其成为高度平衡的二叉树
    /// </summary>
    public class BVH<T> where T: class, IBVHClient
    {
        private const int NullNode = -1;
        private const int DefaultCapacity = 16;
        private const float AabbExtension = 0.1f;
        private const float AabbMultiplier = 2f;
        
        private int _root = NullNode;
        private int _nodeCapacity;
        private int _nodeCount;
        private Dictionary<T, int> _keys;
        private Node[] _nodes;
        private int _freeList;
        
        private readonly Stack<int> _growableStack = new Stack<int>(256);
        
        public BVH() : this(DefaultCapacity)
        {
        }
        
        public BVH(int capacity)
        {
            _root = NullNode;
            _nodeCapacity = capacity;
            _nodeCount = 0;

            _keys = new Dictionary<T, int>(_nodeCapacity);
            _nodes = new Node[_nodeCapacity];

            // 构建为链表的形式
            for (var i = 0; i < _nodeCapacity - 1; i++)
            {
                _nodes[i].next = i + 1;
                _nodes[i].height = -1;
            }

            _nodes[_nodeCapacity - 1].next = NullNode; // 最后一个元素的 next 为 -1
            _nodes[_nodeCapacity - 1].height = -1;
            _freeList = 0;
        }

        /// <summary>
        /// 根节点key
        /// </summary>
        public int rootID => _root;

        /// <summary>
        /// 根节点是否是空
        /// </summary>
        public bool rootIsNull => _root == NullNode;

        /// <summary>
        /// 所有节点的数量
        /// </summary>
        public int nodeCount => _nodeCount;
        
        /// <summary>
        /// 获取当前BVH树的层数/高度
        /// 最底部的子节点为0，每向上一层+1，左右子节点高度不同时，父节点高度为较大的那个子节点的高度+1
        /// </summary>
        public int GetHeight()
        {
            return _root == NullNode ? 0 : _nodes[_root].height;
        }
        
        /// <summary>
        /// 获取最大的平衡因子
        /// 因为采用了AVL树算法，最大的平衡因子不会超过1
        /// </summary>
        public int GetMaxBalance()
        {
            var maxBalance = 0;
            for (var i = 0; i < _nodeCapacity; i++)
            {
                if (_nodes[i].height <= 1)
                {
                    continue;
                }

                Debug.Assert(!_nodes[i].isLeaf);

                var child1 = _nodes[i].child1;
                var child2 = _nodes[i].child2;
                var balance = Mathf.Abs(_nodes[child2].height - _nodes[child1].height);
                maxBalance = Mathf.Max(maxBalance, balance);
            }

            return maxBalance;
        }
        
        /// <summary>
        /// 获取所有节点的面积和根面积的比值
        /// </summary>
        public float GetSurfaceAreaRatio()
        {
            if (_root == NullNode)
            {
                return 0f;
            }

            var rootArea = _nodes[_root].aabb.GetSurfaceArea();

            var totalArea = 0f;
            for (var i = 0; i < _nodeCapacity; i++)
            {
                // 未启用的节点跳过
                if (_nodes[i].height < 0)
                {
                    continue;
                }

                totalArea += _nodes[i].aabb.GetSurfaceArea();
            }

            return totalArea / rootArea;
        }
        
        /// <summary>
        /// 新增一个对象
        /// </summary>
        public int Add(T clientObject)
        {
            var key = AllocateNode();

            Bounds newBounds = clientObject.GetBounds();
            newBounds.Expand(AabbExtension);

            _nodes[key].aabb = newBounds;
            _nodes[key].height = 0;
            _nodes[key].obj = clientObject;

            InsertLeaf(key);

            _keys.Add(clientObject, key);

            return key;
        }
        
        /// <summary>
        /// 在池中分配一个新的节点，如果池子容量不够就进行扩展
        /// </summary>
        private int AllocateNode()
        {
            // _freeList 为 -1 时表示已经没有可分配的节点了，需要扩容
            if (_freeList == NullNode)
            {
                Debug.Assert(_nodeCount == _nodeCapacity);

                // 扩容为原来的2倍
                Node[] oldNodes = _nodes;
                _nodeCapacity *= 2;

                _nodes = new Node[_nodeCapacity];

                for (var i = 0; i < oldNodes.Length; i++)
                {
                    _nodes[i] = oldNodes[i];
                }
                
                // 构建为链表
                for (var i = _nodeCount; i < _nodeCapacity - 1; i++)
                {
                    _nodes[i].next = i + 1;
                    _nodes[i].height = -1;
                }

                _nodes[_nodeCapacity - 1].next = NullNode; // 最后一个元素的 next 为 -1
                _nodes[_nodeCapacity - 1].height = -1;
                _freeList = _nodeCount;
            }

            // 获取后续可用节点并返回
            int nodeId = _freeList;

            _freeList = _nodes[nodeId].next;
            _nodes[nodeId].parent = NullNode;
            _nodes[nodeId].child1 = NullNode;
            _nodes[nodeId].child2 = NullNode;
            _nodes[nodeId].height = 0;
            _nodes[nodeId].next = 0;
            _nodes[nodeId].obj = null;

            ++_nodeCount;

            return nodeId;
        }
        
        private void InsertLeaf(int leaf)
        {
            if (_root == NullNode)
            {
                _root = leaf;
                _nodes[_root].parent = NullNode;
                return;
            }

            // 寻找最佳插入点
            var leafAABB = _nodes[leaf].aabb;
            var index = _root;
            while (!_nodes[index].isLeaf)
            {
                var child1 = _nodes[index].child1;
                var child2 = _nodes[index].child2;

                var area = _nodes[index].aabb.GetSurfaceArea();

                var combinedAABB = _nodes[index].aabb;
                combinedAABB.Encapsulate(leafAABB);
                var combinedArea = combinedAABB.GetSurfaceArea();

                // 该节点和新叶节点创建新父节点的代价
                var cost = 2.0f * combinedArea;

                // 当前节点插入的最小代价
                var inheritanceCost = 2.0f * (combinedArea - area);

                // 降为子节点1的代价
                float cost1;
                if (_nodes[child1].isLeaf)
                {
                    var aabb = leafAABB;
                    aabb.Encapsulate(_nodes[child1].aabb);
                    cost1 = aabb.GetSurfaceArea() + inheritanceCost;
                }
                else
                {
                    var aabb = leafAABB;
                    aabb.Encapsulate(_nodes[child1].aabb);
                    var oldArea = _nodes[child1].aabb.GetSurfaceArea();
                    var newArea = aabb.GetSurfaceArea();
                    cost1 = (newArea - oldArea) + inheritanceCost;
                }

                // 降为子节点2的代价
                float cost2;
                if (_nodes[child2].isLeaf)
                {
                    var aabb = leafAABB;
                    aabb.Encapsulate(_nodes[child2].aabb);
                    cost2 = aabb.GetSurfaceArea() + inheritanceCost;
                }
                else
                {
                    var aabb = leafAABB;
                    aabb.Encapsulate(_nodes[child2].aabb);
                    var oldArea = _nodes[child2].aabb.GetSurfaceArea();
                    var newArea = aabb.GetSurfaceArea();
                    cost2 = newArea - oldArea + inheritanceCost;
                }

                // 代价最小时
                if (cost < cost1 && cost < cost2)
                {
                    break;
                }
                
                // 继续向下查找最小代价
                if (cost1 < cost2)
                {
                    index = child1;
                }
                else
                {
                    index = child2;
                }
            }

            var sibling = index;

            // 创建新的父节点
            var oldParent = _nodes[sibling].parent;
            var newParent = AllocateNode();
            _nodes[newParent].parent = oldParent;
            _nodes[newParent].obj = null;

            var newAABB = leafAABB;
            newAABB.Encapsulate(_nodes[sibling].aabb);
            _nodes[newParent].aabb = newAABB;

            _nodes[newParent].height = _nodes[sibling].height + 1;

            if (oldParent != NullNode)
            {
                // oldParent 不是根节点时
                if (_nodes[oldParent].child1 == sibling)
                {
                    _nodes[oldParent].child1 = newParent;
                }
                else
                {
                    _nodes[oldParent].child2 = newParent;
                }
                
                _nodes[newParent].child1 = sibling;
                _nodes[newParent].child2 = leaf;
                _nodes[sibling].parent = newParent;
                _nodes[leaf].parent = newParent;
            }
            else
            {
                // oldParent 是根节点时
                _nodes[newParent].child1 = sibling;
                _nodes[newParent].child2 = leaf;
                _nodes[sibling].parent = newParent;
                _nodes[leaf].parent = newParent;
                _root = newParent;
            }

            // 向上遍历，重新计算高度和包围盒
            index = _nodes[leaf].parent;
            while (index != NullNode)
            {
                index = Balance(index);

                var child1 = _nodes[index].child1;
                var child2 = _nodes[index].child2;

                Debug.Assert(child1 != NullNode);
                Debug.Assert(child2 != NullNode);

                _nodes[index].height = 1 + Mathf.Max(_nodes[child1].height, _nodes[child2].height);

                var aabb = _nodes[child1].aabb;
                aabb.Encapsulate(_nodes[child2].aabb);
                _nodes[index].aabb = aabb;

                index = _nodes[index].parent;
            }
        }
        
        /// <summary>
        /// AVL树算法，通过左旋和右旋高度平衡当前二叉树
        /// </summary>
        private int Balance(int iA)
        {
            Debug.Assert(iA != NullNode);

            if (_nodes[iA].isLeaf || _nodes[iA].height < 2)
            {
                return iA;
            }

            var iB = _nodes[iA].child1;
            var iC = _nodes[iA].child2;

            Debug.Assert(0 <= iB && iB < _nodeCapacity);
            Debug.Assert(0 <= iC && iC < _nodeCapacity);

            var balance = _nodes[iC].height - _nodes[iB].height;

            // 旋转C
            if (balance > 1)
            {
                var iF = _nodes[iC].child1;
                var iG = _nodes[iC].child2;

                Debug.Assert(0 <= iF && iF < _nodeCapacity);
                Debug.Assert(0 <= iG && iG < _nodeCapacity);

                _nodes[iC].child1 = iA;
                _nodes[iC].parent = _nodes[iA].parent;
                _nodes[iA].parent = iC;

                if (_nodes[iC].parent != NullNode)
                {
                    if (_nodes[_nodes[iC].parent].child1 == iA)
                    {
                        _nodes[_nodes[iC].parent].child1 = iC;
                    }
                    else
                    {
                        Debug.Assert(_nodes[_nodes[iC].parent].child2 == iA);
                        _nodes[_nodes[iC].parent].child2 = iC;
                    }
                }
                else
                {
                    _root = iC;
                }

                if (_nodes[iF].height > _nodes[iG].height)
                {
                    _nodes[iC].child2 = iF;
                    _nodes[iA].child2 = iG;
                    _nodes[iG].parent = iA;

                    var bounds = _nodes[iB].aabb;
                    bounds.Encapsulate(_nodes[iG].aabb);
                    _nodes[iA].aabb = bounds;

                    bounds = _nodes[iA].aabb;
                    bounds.Encapsulate(_nodes[iF].aabb);
                    _nodes[iC].aabb = bounds;

                    _nodes[iA].height = 1 + Mathf.Max(_nodes[iB].height, _nodes[iG].height);
                    _nodes[iC].height = 1 + Mathf.Max(_nodes[iA].height, _nodes[iF].height);
                }
                else
                {
                    _nodes[iC].child2 = iG;
                    _nodes[iA].child2 = iF;
                    _nodes[iF].parent = iA;

                    var bounds = _nodes[iB].aabb;
                    bounds.Encapsulate(_nodes[iF].aabb);
                    _nodes[iA].aabb = bounds;

                    bounds = _nodes[iA].aabb;
                    bounds.Encapsulate(_nodes[iG].aabb);
                    _nodes[iC].aabb = bounds;

                    _nodes[iA].height = 1 + Mathf.Max(_nodes[iB].height, _nodes[iF].height);
                    _nodes[iC].height = 1 + Mathf.Max(_nodes[iA].height, _nodes[iG].height);
                }

                return iC;
            }

            // 旋转B
            if (balance < -1)
            {
                var iD = _nodes[iB].child1;
                var iE = _nodes[iB].child2;

                Debug.Assert(0 <= iD && iD < _nodeCapacity);
                Debug.Assert(0 <= iE && iE < _nodeCapacity);

                _nodes[iB].child1 = iA;
                _nodes[iB].parent = _nodes[iA].parent;
                _nodes[iA].parent = iB;

                if (_nodes[iB].parent != NullNode)
                {
                    if (_nodes[_nodes[iB].parent].child1 == iA)
                    {
                        _nodes[_nodes[iB].parent].child1 = iB;
                    }
                    else
                    {
                        Debug.Assert(_nodes[_nodes[iB].parent].child2 == iA);
                        _nodes[_nodes[iB].parent].child2 = iB;
                    }
                }
                else
                {
                    _root = iB;
                }

                if (_nodes[iD].height > _nodes[iE].height)
                {
                    _nodes[iB].child2 = iD;
                    _nodes[iA].child1 = iE;
                    _nodes[iE].parent = iA;

                    var bounds = _nodes[iC].aabb;
                    bounds.Encapsulate(_nodes[iE].aabb);
                    _nodes[iA].aabb = bounds;

                    bounds = _nodes[iA].aabb;
                    bounds.Encapsulate(_nodes[iD].aabb);
                    _nodes[iB].aabb = bounds;

                    _nodes[iA].height = 1 + Mathf.Max(_nodes[iC].height, _nodes[iE].height);
                    _nodes[iB].height = 1 + Mathf.Max(_nodes[iA].height, _nodes[iD].height);
                }
                else
                {
                    _nodes[iB].child2 = iE;
                    _nodes[iA].child1 = iD;
                    _nodes[iD].parent = iA;

                    var bounds = _nodes[iC].aabb;
                    bounds.Encapsulate(_nodes[iD].aabb);
                    _nodes[iA].aabb = bounds;

                    bounds = _nodes[iA].aabb;
                    bounds.Encapsulate(_nodes[iE].aabb);
                    _nodes[iB].aabb = bounds;

                    _nodes[iA].height = 1 + Mathf.Max(_nodes[iC].height, _nodes[iD].height);
                    _nodes[iB].height = 1 + Mathf.Max(_nodes[iA].height, _nodes[iE].height);
                }

                return iB;
            }

            return iA;
        }
        
        /// <summary>
        /// 更新指定对象
        /// 当已添加对象的位置或包围盒发生变化，需要更新当前BVH树时调用该方法刷新BVH树
        /// </summary>
        /// <returns>更新的对象层（父节点改变）发生变化时返回true，否则返回false</returns>
        public bool Update(T clientObject)
        {
            int key = 0;
            Debug.Assert(_keys.TryGetValue(clientObject, out key));
            Debug.Assert(0 <= key && key < _nodeCapacity);
            Debug.Assert(_nodes[key].isLeaf);

            var aabb = clientObject.GetBounds();
            var displacement = clientObject.GetPosition() - clientObject.GetPreviousPosition();

            // 扩大AABB
            var fatAABB = aabb;
            fatAABB.Expand(AabbExtension);

            // 预测AABB的变化
            var d = AabbMultiplier * displacement;

            if (d.x < 0f)
            {
                fatAABB.min += d.x * Vector3.right;
            }
            else
            {
                fatAABB.max += d.x * Vector3.right;
            }

            if (d.y < 0f)
            {
                fatAABB.min += d.y * Vector3.up;
            }
            else
            {
                fatAABB.max += d.y * Vector3.up;
            }

            if (d.z < 0f)
            {
                fatAABB.min += d.z * Vector3.forward;
            }
            else
            {
                fatAABB.max += d.z * Vector3.forward;
            }

            var treeAABB = _nodes[key].aabb;
            if (treeAABB.Contains(aabb))
            {
                var r = AabbMultiplier * Vector3.one;

                // AABB包含对象，但是AABB较大的情况
                var min = fatAABB.min - 4f * r;
                var max = fatAABB.max + 4f * r;

                var hugeAABB = new Bounds();
                hugeAABB.SetMinMax(min, max);

                if (hugeAABB.Contains(treeAABB))
                {
                    // 被父级AABB包含，不需要更新树的结构
                    return false;
                }

                // 其它情况，AABB过大了需要缩小
            }

            RemoveLeaf(key);

            _nodes[key].aabb = fatAABB;

            InsertLeaf(key);

            return true;
        }
        
        /// <summary>
        /// 移除指定对象
        /// </summary>
        public void Remove(T clientObject)
        {
            int key = 0;
            Debug.Assert(_keys.TryGetValue(clientObject, out key));
            Debug.Assert(0 <= key && key < _nodeCapacity);
            Debug.Assert(_nodes[key].isLeaf);

            _keys.Remove(clientObject);

            RemoveLeaf(key);
            FreeNode(key);
        }
        
        private void RemoveLeaf(int leaf)
        {
            if (leaf == _root)
            {
                _root = NullNode;
                return;
            }

            var parent = _nodes[leaf].parent;
            var grandParent = _nodes[parent].parent;
            int sibling;

            if (_nodes[parent].child1 == leaf)
            {
                sibling = _nodes[parent].child2;
            }
            else
            {
                sibling = _nodes[parent].child1;
            }

            if (grandParent != NullNode)
            {
                // 销毁父级并连接到祖父级
                if (_nodes[grandParent].child1 == parent)
                {
                    _nodes[grandParent].child1 = sibling;
                }
                else
                {
                    _nodes[grandParent].child2 = sibling;
                }

                _nodes[sibling].parent = grandParent;
                FreeNode(parent);

                // 校正父级到根节点的包围盒数据
                var index = grandParent;
                while (index != NullNode)
                {
                    index = Balance(index);

                    var child1 = _nodes[index].child1;
                    var child2 = _nodes[index].child2;

                    var aabb = _nodes[child1].aabb;
                    aabb.Encapsulate(_nodes[child2].aabb);
                    _nodes[index].aabb = aabb;
                    _nodes[index].height = 1 + Mathf.Max(_nodes[child1].height, _nodes[child2].height);

                    index = _nodes[index].parent;
                }
            }
            else
            {
                _root = sibling;
                _nodes[sibling].parent = NullNode;
                FreeNode(parent);
            }
        }
        
        private void FreeNode(int key)
        {
            Debug.Assert(0 <= key && key < _nodeCapacity);
            Debug.Assert(0 < _nodeCount);

            _nodes[key].next = _freeList;
            _nodes[key].height = -1;
            _freeList = key;
            --_nodeCount;
        }
        
        /// <summary>
        /// 移除指定key的对象
        /// </summary>
        public void Remove(int key)
        {
            Debug.Assert(0 <= key && key < _nodeCapacity);
            Debug.Assert(_nodes[key].isLeaf);

            var node = _nodes[key];

            _keys.Remove(node.obj);

            RemoveLeaf(key);
            FreeNode(key);
        }

        /// <summary>
        /// 获取指定对象的索引
        /// </summary>
        public int GetKey(T clientObject)
        {
            int key = 0;
            Debug.Assert(_keys.TryGetValue(clientObject, out key));
            return key;
        }

        /// <summary>
        /// 获取指定对象的节点
        /// </summary>
        public Node GetNode(T clientObject)
        {
            int key = 0;
            Debug.Assert(_keys.TryGetValue(clientObject, out key));
            return GetNode(key);
        }

        /// <summary>
        /// 获取指定key的节点
        /// </summary>
        public Node GetNode(int key)
        {
            Debug.Assert(0 <= key && key < _nodeCapacity);
            return _nodes[key];
        }

        /// <summary>
        /// 尝试获取指定key的节点
        /// </summary>
        public bool TryGetNode(int key, out Node node)
        {
            node = default(Node);

            if (key < 0 || key >= _nodeCapacity || key == NullNode)
            {
                return false;
            }

            node = _nodes[key];

            return true;
        }

        /// <summary>
        /// 枚举所有节点，包括叶节点和内部节点，并返回节点本身及其在树中的深度
        /// </summary>
        public IEnumerable<(Node, int)> EnumerateNodes()
        {
            foreach ((Node, int) node in EnumerateNodes(_root, 0))
            {
                yield return node;
            }
        }

        private IEnumerable<(Node, int)> EnumerateNodes(int startPoint, int currentDepth)
        {
            Debug.Assert(0 <= startPoint && startPoint < _nodeCapacity);
            Debug.Assert(startPoint != NullNode);

            var node = _nodes[startPoint];

            yield return (node, currentDepth);

            if (!node.isLeaf)
            {
                var child1 = node.child1;

                if (child1 != NullNode)
                {
                    foreach (var child1Node in EnumerateNodes(child1, currentDepth + 1))
                    {
                        yield return child1Node;
                    }
                }

                var child2 = node.child2;

                if (child2 != NullNode)
                {
                    foreach (var child2Node in EnumerateNodes(child2, currentDepth + 1))
                    {
                        yield return child2Node;
                    }
                }
            }
        }
        
        /// <summary>
        /// 自下到上重新构建当前的BVH树为最优树
        /// 注意该操作时间代价较大
        /// </summary>
        public void Rebuild()
        {
            var nodes = new int[_nodeCount];
            var count = 0;

            // 释放掉已有的数据
            for (var i = 0; i < _nodeCapacity; i++)
            {
                if (_nodes[i].height < 0)
                {
                    continue;
                }

                if (_nodes[i].isLeaf)
                {
                    _nodes[i].parent = NullNode;
                    nodes[count] = i;
                    ++count;
                }
                else
                {
                    FreeNode(i);
                }
            }

            while (count > 1)
            {
                var minCost = float.MaxValue;
                int iMin = -1, jMin = -1;
                for (var i = 0; i < count; ++i)
                {
                    var aabbi = _nodes[nodes[i]].aabb;

                    for (var j = i + 1; j < count; ++j)
                    {
                        var aabbj = _nodes[nodes[j]].aabb;
                        var b = aabbi;
                        b.Encapsulate(aabbj);

                        var cost = b.GetSurfaceArea();
                        if (cost < minCost)
                        {
                            iMin = i;
                            jMin = j;
                            minCost = cost;
                        }
                    }
                }

                var index1 = nodes[iMin];
                var index2 = nodes[jMin];

                var parentIndex = AllocateNode();
                _nodes[parentIndex].child1 = index1;
                _nodes[parentIndex].child2 = index2;
                _nodes[parentIndex].height = 1 + Mathf.Max(_nodes[index1].height, _nodes[index2].height);

                var newAABB = _nodes[index1].aabb;
                newAABB.Encapsulate(_nodes[index2].aabb);
                _nodes[parentIndex].aabb = newAABB;

                _nodes[parentIndex].parent = NullNode;

                _nodes[index1].parent = parentIndex;
                _nodes[index2].parent = parentIndex;

                nodes[jMin] = nodes[count - 1];
                nodes[iMin] = parentIndex;
                --count;
            }

            _root = nodes[0];

            Validate();
        }
        
        /// <summary>
        /// 枚举出所有被参数bounds包含和相交的叶子节点
        /// </summary>
        public IEnumerable<Node> EnumerateOverlappingLeafNodes(Bounds bounds)
        {
            _growableStack.Clear();
            _growableStack.Push(_root);

            while (_growableStack.Count > 0)
            {
                var nodeId = _growableStack.Pop();

                if (nodeId == NullNode)
                {
                    continue;
                }

                if (_nodes[nodeId].aabb.Intersects(bounds))
                {
                    if (_nodes[nodeId].isLeaf)
                    {
                        yield return _nodes[nodeId];
                    }
                    else
                    {
                        _growableStack.Push(_nodes[nodeId].child1);
                        _growableStack.Push(_nodes[nodeId].child2);
                    }
                }
            }
        }
        
        /// <summary>
        /// 判断被射线射中的叶子节点，如果有多个节点可以被射线射中则hit包含距离射线最近的节点
        /// </summary>
        public bool Raycast(Ray ray, out Node hit)
        {
            return Raycast(ray, _root, out hit);
        }
        
        private bool Raycast(Ray ray, int startPoint, out Node hit)
        {
            Debug.Assert(0 <= startPoint && startPoint < _nodeCapacity);
            Debug.Assert(startPoint != NullNode);
            
            hit = default;
            var node = _nodes[startPoint];
            
            if (node.isLeaf)
            {
                hit = node;
                return true;
            }
            
            if (TryGetNode(node.child1, out Node node1) && node1.aabb.IntersectRay(ray) && Raycast(ray, node.child1, out hit))
            {
                return true;
            }
            
            if (TryGetNode(node.child2, out Node node2) && node2.aabb.IntersectRay(ray) && Raycast(ray, node.child2, out hit))
            {
                return true;
            }
            
            return false;
        }
        
        /// <summary>
        /// 获取指定对象在BVH中的包围盒，会比自身返回的包围盒大一点（AabbExtension常量）
        /// </summary>
        public Bounds GetPaddedBounds(T clientObject)
        {
            int key = 0;
            Debug.Assert(_keys.TryGetValue(clientObject, out key));
            return GetPaddedBounds(key);
        }
        
        /// <summary>
        /// 获取指定索引在BVH中的包围盒，会比自身返回的包围盒大一点（AabbExtension常量）
        /// </summary>
        public Bounds GetPaddedBounds(int key)
        {
            Debug.Assert(0 <= key && key < _nodeCapacity);
            return _nodes[key].aabb;
        }
        
        /// <summary>
        /// 移动所有节点的位置
        /// 移动位置公式为：position -= newOrigin
        /// </summary>
        public void ShiftOrigin(Vector3 newOrigin)
        {
            for (var i = 0; i < _nodeCapacity; i++)
            {
                _nodes[i].aabb.min -= newOrigin;
                _nodes[i].aabb.max -= newOrigin;
            }
        }
        
        /// <summary>
        /// 验证当前BVH树的数据有效性
        /// </summary>
        public void Validate()
        {
            ValidateStructure(_root);
            ValidateMetrics(_root);

            var freeCount = 0;
            var freeIndex = _freeList;
            while (freeIndex != NullNode)
            {
                Debug.Assert(0 <= freeIndex && freeIndex < _nodeCapacity);
                freeIndex = _nodes[freeIndex].next;
                ++freeCount;
            }

            Debug.Assert(GetHeight() == ComputeHeight());

            Debug.Assert(_nodeCount + freeCount == _nodeCapacity);
        }
        
        private void ValidateStructure(int index)
        {
            if (index == NullNode)
            {
                return;
            }

            if (index == _root)
            {
                Debug.Assert(_nodes[index].parent == NullNode);
            }

            var child1 = _nodes[index].child1;
            var child2 = _nodes[index].child2;

            if (_nodes[index].isLeaf)
            {
                Debug.Assert(child1 == NullNode);
                Debug.Assert(child2 == NullNode);
                Debug.Assert(_nodes[index].height == 0);
                return;
            }

            Debug.Assert(0 <= child1 && child1 < _nodeCapacity);
            Debug.Assert(0 <= child2 && child2 < _nodeCapacity);

            Debug.Assert(_nodes[child1].parent == index);
            Debug.Assert(_nodes[child2].parent == index);

            ValidateStructure(child1);
            ValidateStructure(child2);
        }
        
        private void ValidateMetrics(int index)
        {
            if (index == NullNode)
            {
                return;
            }

            var child1 = _nodes[index].child1;
            var child2 = _nodes[index].child2;

            if (_nodes[index].isLeaf)
            {
                Debug.Assert(child1 == NullNode);
                Debug.Assert(child2 == NullNode);
                Debug.Assert(_nodes[index].height == 0);
                return;
            }

            Debug.Assert(0 <= child1 && child1 < _nodeCapacity);
            Debug.Assert(0 <= child2 && child2 < _nodeCapacity);

            var height1 = _nodes[child1].height;
            var height2 = _nodes[child2].height;
            var height = 1 + Mathf.Max(height1, height2);
            Debug.Assert(_nodes[index].height == height);

            var aabb = _nodes[child1].aabb;
            aabb.Encapsulate(_nodes[child2].aabb);

            Debug.Assert(aabb.min == _nodes[index].aabb.min);
            Debug.Assert(aabb.max == _nodes[index].aabb.max);

            ValidateMetrics(child1);
            ValidateMetrics(child2);
        }
        
        private int ComputeHeight()
        {
            return ComputeHeight(_root);
        }

        private int ComputeHeight(int nodeId)
        {
            Debug.Assert(0 <= nodeId && nodeId < _nodeCapacity);

            if (_nodes[nodeId].isLeaf)
            {
                return 0;
            }

            var height1 = ComputeHeight(_nodes[nodeId].child1);
            var height2 = ComputeHeight(_nodes[nodeId].child2);
            return 1 + Mathf.Max(height1, height2);
        }
        
        /// <summary>
        /// 二叉树节点，为了保证内存连续加大缓存命中率所以设计为结构体
        /// </summary>
        public struct Node
        {
            public bool isLeaf => child1 == NullNode;
            public int child1;
            public int child2;
            public int height; // 二叉树的层数/高度，即用来配合AVL树算法计算平衡因子的层数/高度，最底部的子节点为0，每向上一层+1，左右子节点高度不同时，父节点高度为较大的那个子节点的高度+1
            public int parent;
            public int next;
            public Bounds aabb;
            public T obj;
        }
    }
}
